#Copyright (C) Nial Peters 2012
#
#This file is part of AvoScan.
#
#AvoScan is free software: you can redistribute it and/or modify
#it under the terms of the GNU General Public License as published by
#the Free Software Foundation, either version 3 of the License, or
#(at your option) any later version.
#
#AvoScan is distributed in the hope that it will be useful,
#but WITHOUT ANY WARRANTY; without even the implied warranty of
#MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
#GNU General Public License for more details.
#
#You should have received a copy of the GNU General Public License
#along with AvoScan.  If not, see <http://www.gnu.org/licenses/>.
"""
The compass module provides an interface for the CMPS10 electronic compass chip
which is mounted inside some of the AvoScanner units. This module is still largely
unfinished and a lot of work is needed to accurately calibrate the compass before 
it is really useful.
"""
import serial
import time
from avoscan import ports

def byte2int(bstr, width):
    """
    Convert a byte string into a signed integer value of specified width.
    """

    val = sum(ord(b) << 8*n for (n, b) in enumerate(reversed(bstr)))
    if val >= (1 << (width - 1)):
        val = val - (1 << width)
    return val


class Compass:
    """
    The Compass class provides an easy to use interface for the CMPS10
    electronic compass chip inside some of the AvoScanner units.
    """
    def __init__(self, scanner):
        
        port_manager = ports.SerialPortManager(scanner)
        
        self.port = serial.Serial(port_manager.get_compass_port(), 
                                  baudrate=9600, 
                                  stopbits=serial.STOPBITS_TWO,
                                  parity = serial.PARITY_NONE,                               
                                  timeout=1)
        
    def close(self):
        """
        Close the serial connection to the compass chip.
        """
        self.port.close()
    
    
    def get_bearing(self):
        """
        Returns the current compass bearing (degrees from North), 
        with a 0.1 degree accuracy. Note that the compass will need
        to be accurately calibrated before this value is actually 
        meaningful.
        """
        self.port.write(chr(0x13))
        byte_str = self.port.read(2)
        return float(byte2int(byte_str,16)/10)
    
    
    def get_pitch(self):
        """
        Returns the pitch angle of the compass in degrees (+-0-85 deg.).
        """
        self.port.write(chr(0x14))
        byte_str = self.port.read(1)
        return byte2int(byte_str, 8)
    
    
    def get_roll(self):
        """
        Returns the roll angle of the compass in degrees (+-0-85 deg.).
        """
        self.port.write(chr(0x15))
        byte_str = self.port.read(1)
        return byte2int(byte_str, 8)
    
    
    def get_scan_type(self):
        """
        Returns either 'Vertical' or 'Horizontal' depending on the 
        orientation of the scanner. Any scan with a pitch angle of less than 20 degrees
        is considered to be vertical.
        """
        if abs(self.get_pitch()) < 20:
            return "Vertical"
        else:
            return "Horizontal"
    
    
    def reset_factory_calibration(self):
        """
        Resets the factory calibration on the compass.
        """
        self.port.write(chr(0x6A))
        time.sleep(0.1)
        self.port.write(chr(0x7C))
        time.sleep(0.1)
        self.port.write(chr(0x81))
        